/**
 * @brief Convex Quadratic Programming library
 *
 * From:
 * http://ra.uni-trier.de/~huebner/software.html
 *
 * ----------------------------------------------------------------------
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 * ----------------------------------------------------------------------
 *
 * @author Ewgenij Hübner
 *
 * $$
 */

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>

#include "gsl_cqp.h"
#include "initial_point.h"

/*
static void
print_vectors(const gsl_vector*,const gsl_vector*,const gsl_vector*,const
gsl_vector*);
*/

static double compute_gap_infeasible_points(const gsl_cqp_data *cqp,
                                            const gsl_vector *x,
                                            const gsl_vector *y,
                                            const gsl_vector *z);

static int compute_residuals(const gsl_cqp_data *cqp, const gsl_vector *x,
                             const gsl_vector *y, const gsl_vector *z,
                             const gsl_vector *s, gsl_vector *r);

static int build_kkt_matrix(const gsl_cqp_data *cqp, const gsl_vector *z,
                            const gsl_vector *s, gsl_matrix *kkt_matrix);

double step_length(const gsl_vector *s, const gsl_vector *z,
                   const gsl_vector *delta_s, const gsl_vector *delta_z);

double gsl_vector_max_norm(const gsl_vector *v);

double gsl_matrix_max_norm(const gsl_matrix *M);

typedef struct {
    size_t n;      /* dimension of the problem*/
    size_t me;     /* number of the equality constraints*/
    size_t mi;     /* number of the inequality constraints */
    gsl_vector *s; /* Slack vector for the constrain: Cx+s = d */

    gsl_matrix *kkt_matrix;
    gsl_vector *r; /* the vector of the right-hand side in the kkt-system:
                      r=(r_Q,r_A,r_C) */

    double tau; /* a constant for the Mehrotra's heuristic */

    /* gondzio's parameters */
    size_t k_max; /*maximal number of corrections */
    double beta_min,
        beta_max; /* relative threshold values for outlier compl. products */
    double delta_alpha; /* the required increase of stepsize */
    double gamma;       /* the minimum acceptable increase of stepsize */

    double data_norm; /* norm of the problem data */

} mg_pdip_state;

static int mg_pdip_alloc(void *vstate, size_t n, size_t me, size_t mi) {
    mg_pdip_state *state = (mg_pdip_state *)vstate;

    state->s = gsl_vector_alloc(mi);
    if (state->s == 0) {
        GSL_ERROR_VAL("failed to initialize space for the slack vector s",
                      GSL_ENOMEM, 0);
    }

    state->kkt_matrix = gsl_matrix_calloc(n + me + mi, n + me + mi);
    if (state->kkt_matrix == 0) {
        gsl_vector_free(state->s);
        GSL_ERROR_VAL("failed to initialize space for the KKT-Matrix",
                      GSL_ENOMEM, 0);
    }

    state->r = gsl_vector_alloc(state->kkt_matrix->size1);
    if (state->r == 0) {
        gsl_matrix_free(state->kkt_matrix);
        gsl_vector_free(state->s);
        GSL_ERROR_VAL(
            "failed to initialize space for right-hand side of the KKT-System",
            GSL_ENOMEM, 0);
    }

    return GSL_SUCCESS;
}

static int mg_pdip_set(void *vstate, const gsl_cqp_data *cqp, gsl_vector *x,
                       gsl_vector *y, gsl_vector *z, double *gap,
                       double *residuals_norm, double *data_norm,
                       double *infeasibility, double *infeasibility_min) {
    int status = GSL_SUCCESS;
    // size_t i, j = 0; //, debug=0;

    mg_pdip_state *state = (mg_pdip_state *)vstate;

    /* Initial points */
    if (cqp->A->size1 != 0)
        status = pdip_initial_point_feasible_x(cqp->A, cqp->b, x);
    else
        gsl_vector_set_zero(x);

    if (status != GSL_SUCCESS) {
        return status;
    }

    status = pdip_initial_point_feasible_s(cqp->C, cqp->d, x, state->s);
    if (status != GSL_SUCCESS) {
        return status;
    }

    if (cqp->A->size1 != 0) {
        status = pdip_initial_point_y(cqp->Q, cqp->q, cqp->A, x, y);
        if (status != GSL_SUCCESS) {
            return status;
        }
    }

    status = pdip_initial_point_z(z);
    if (status != GSL_SUCCESS) {
        return status;
    }

    status = pdip_initial_point_strict_feasible(z, state->s);
    if (status != GSL_SUCCESS) {
        return status;
    }

    /* Dualtity gap */
    status = gsl_blas_ddot(z, state->s, gap);
    if (status != GSL_SUCCESS) {
        return status;
    }

    *gap /= (double)cqp->C->size1;

    status = build_kkt_matrix(cqp, z, state->s, state->kkt_matrix);
    if (status != GSL_SUCCESS) {
        return status;
    }

    state->tau = 3.0;

    /* for the convergence conditions */
    /* data_norm = ||Q,A,C,q,b,d||_{infinity} */
    *data_norm = gsl_matrix_max_norm(cqp->Q);

    if (cqp->A->size1 != 0) {
        *data_norm = GSL_MAX_DBL(*data_norm, gsl_matrix_max_norm(cqp->A));
    }

    *data_norm = GSL_MAX_DBL(*data_norm, gsl_matrix_max_norm(cqp->C));

    *data_norm = GSL_MAX_DBL(*data_norm, gsl_vector_max_norm(cqp->q));

    if (cqp->A->size1 != 0) {
        *data_norm = GSL_MAX_DBL(*data_norm, gsl_vector_max_norm(cqp->b));
    }

    *data_norm = GSL_MAX_DBL(*data_norm, gsl_vector_max_norm(cqp->d));

    state->data_norm = *data_norm;

    /* ||r||_{infinity}=||r_Q, r_A, r_C||_{infinity} */
    status = compute_residuals(cqp, x, y, z, state->s, state->r);
    *residuals_norm = gsl_vector_max_norm(state->r);

    *infeasibility =
        (*residuals_norm + compute_gap_infeasible_points(cqp, x, y, z)) /
        (state->data_norm);
    *infeasibility_min = *infeasibility;

    /* Gondzio's paameters */
    state->k_max = 0;
    state->beta_min = 0.1;
    state->beta_max = 10.0;
    state->delta_alpha = 0.1;
    state->gamma = 0.1;

    //	if(debug)
    //	{
    //		printf("\nStart points:\n");
    //		print_vectors(x, y, z, state->s);
    //		printf("\nDuality gap: %e\n",*gap);
    //
    //		printf("\nKKT-matrix:\n");
    //		for(i=0; i<state->kkt_matrix->size1; i++)
    //		{
    //			for(j=0; j<state->kkt_matrix->size2; j++)
    //				printf("%4.2f
    //",gsl_matrix_get(state->kkt_matrix,i,j));
    //			printf("\n");
    //		}
    //		printf("\n");
    //	}

    return GSL_SUCCESS;
}

static int mg_pdip_iterate(void *vstate, const gsl_cqp_data *cqp, gsl_vector *x,
                           gsl_vector *y, gsl_vector *z, double *gap,
                           double *residuals_norm, double *infeasibility,
                           double *infeasibility_min) {
    size_t i, j = 0;  //, debug=0;
    int signum;
    double sigma, alpha, alpha_gondzio;
    double tmp_v, tmp_vt, tmp_d;

    gsl_vector_view r_block, delta_block;

    mg_pdip_state *state = (mg_pdip_state *)vstate;

    gsl_permutation *p;

    gsl_vector *delta;
    gsl_vector *delta_s;

    gsl_vector *r_zs;

    gsl_vector *delta_gondzio;
    gsl_vector *delta_s_gondzio;

    p = gsl_permutation_alloc(state->kkt_matrix->size1);
    if (p == 0) {
        GSL_ERROR_VAL("failed to initialize space for permutation vector",
                      GSL_ENOMEM, 0);
    }

    delta = gsl_vector_alloc(state->kkt_matrix->size2);
    if (delta == 0) {
        gsl_permutation_free(p);
        GSL_ERROR_VAL("failed to initialize space for predictor step",
                      GSL_ENOMEM, 0);
    }
    delta_gondzio = gsl_vector_alloc(state->kkt_matrix->size2);
    if (delta_gondzio == 0) {
        gsl_vector_free(delta);
        gsl_permutation_free(p);
        GSL_ERROR_VAL("failed to initialize space for predictor step",
                      GSL_ENOMEM, 0);
    }

    delta_s_gondzio = gsl_vector_alloc(z->size);
    if (delta_s_gondzio == 0) {
        gsl_vector_free(delta_gondzio);
        gsl_vector_free(delta);
        gsl_permutation_free(p);
        GSL_ERROR_VAL("failed to initialize space for LM s", GSL_ENOMEM, 0);
    }

    delta_s = gsl_vector_alloc(z->size);
    if (delta_s == 0) {
        gsl_vector_free(delta_s_gondzio);
        gsl_vector_free(delta_gondzio);
        gsl_vector_free(delta);
        gsl_permutation_free(p);
        GSL_ERROR_VAL("failed to initialize space for LM s", GSL_ENOMEM, 0);
    }

    r_zs = gsl_vector_alloc(z->size);
    if (r_zs == 0) {
        gsl_vector_free(delta_s);
        gsl_vector_free(delta_s_gondzio);
        gsl_vector_free(delta_gondzio);
        gsl_vector_free(delta);
        gsl_permutation_free(p);
        GSL_ERROR_VAL("failed to initialize space for LM s", GSL_ENOMEM, 0);
    }

    /* the right-hand side of the KKT-system: r = -(r_Q, r_A, r_C+Z^{-1}*r_zs)
     */
    /* the vecors of variables: delta = (delta_x, -delta_y, -delta_z) */
    /* delta_s for the slack variable s: delta_s = Z^{-1}(-r_zs - S*delta_z) */

    /********* Predictor Step ******************/
    /* in the predictor step: r_zs = ZSe */
    r_block = gsl_vector_subvector(state->r, cqp->Q->size1 + cqp->A->size1,
                                   cqp->C->size1);
    gsl_blas_daxpy(1.0, state->s, &r_block.vector);
    gsl_blas_dscal(-1.0, state->r);

    /* solve the KKT-system: */
    /* evaluate the LU-decomposition of the KKT-matrix*/
    gsl_linalg_LU_decomp(state->kkt_matrix, p, &signum);
    /* find the predictor step */
    gsl_linalg_LU_solve(state->kkt_matrix, p, state->r, delta);

    for (i = 0; i < delta_s->size; i++) {
        /* find delta_s for the slack variable s: delta_s = Z^{-1}(-r_zs -
         * S*delta_z) */
        gsl_vector_set(
            delta_s, i,
            gsl_vector_get(state->s, i) *
                (gsl_vector_get(delta, cqp->Q->size2 + cqp->A->size1 + i) /
                     gsl_vector_get(z, i) -
                 1.0));
    }

    /* find the stepsize of the predictor step */
    delta_block = gsl_vector_subvector(delta, cqp->Q->size1 + cqp->A->size1,
                                       cqp->C->size1);
    alpha = step_length(state->s, z, delta_s, &delta_block.vector);

    //	if(debug)
    //	{
    //		printf("\n *** Predictor Step ***\n");
    //		printf("\nthe right-hand side of the KKT-system:\n");
    //		for(i=0; i<state->r->size; i++)
    //			printf("r[%lu]=%f ",i,gsl_vector_get(state->r,i));
    //		printf("\n");
    //		printf("\nsolution (delta_x,-delta_y,-delta_z):\n");
    //		for(i=0; i<delta->size; i++)
    //			printf("%6.3f ",gsl_vector_get(delta,i));
    //		printf("\n");
    //		printf("\ndelta_s\n");
    //		for(i=0; i<delta_s->size; i++)
    //			printf("%6.3f ",gsl_vector_get(delta_s,i));
    //		printf("\n");
    //		printf("the stepsize for the predictor step=%f\n",alpha);
    //	}

    /************  Evaluation of the centering parameter sigma ***************/
    /* sigma = (gap_aff/gap)^tau */
    sigma = 0.0;
    for (i = 0; i < z->size; i++) {
        sigma +=
            (gsl_vector_get(z, i) -
             alpha * gsl_vector_get(delta, cqp->Q->size2 + cqp->A->size1 + i)) *
            (gsl_vector_get(state->s, i) + alpha * gsl_vector_get(delta_s, i));
    }
    sigma /= (cqp->C->size1 * (*gap));
    sigma = pow(sigma, state->tau);

    //	if(debug)
    //		printf("the centering parameter sigma =%f\n",sigma);

    /************  Corrector Step ******************/

    /* modify the right-hand side of the kkt-system in order to find the
     * Mehrotra's corrector step */

    r_block = gsl_vector_subvector(state->r, cqp->Q->size1 + cqp->A->size1,
                                   cqp->C->size1);
    for (i = 0; i < cqp->C->size1; i++) {
        tmp_d = -sigma * (*gap) -
                gsl_vector_get(delta, cqp->Q->size2 + cqp->A->size1 + i) *
                    gsl_vector_get(delta_s, i);
        gsl_vector_set(
            r_zs, i,
            gsl_vector_get(z, i) * gsl_vector_get(state->s, i) + tmp_d);
        gsl_vector_set(
            &r_block.vector, i,
            gsl_vector_get(&r_block.vector, i) - tmp_d / gsl_vector_get(z, i));
    }

    /* find the corrector step */
    gsl_linalg_LU_solve(state->kkt_matrix, p, state->r, delta);

    for (i = 0; i < delta_s->size; i++) {
        /* delta_s = Z^{-1}*(-r_zs-S*delta_z) */
        gsl_vector_set(
            delta_s, i,
            (-gsl_vector_get(r_zs, i) +
             gsl_vector_get(delta, cqp->Q->size2 + cqp->A->size1 + i) *
                 gsl_vector_get(state->s, i)) /
                gsl_vector_get(z, i));
    }

    /* evaluate the stepsize */
    delta_block = gsl_vector_subvector(delta, cqp->Q->size1 + cqp->A->size1,
                                       cqp->C->size1);
    alpha = step_length(state->s, z, delta_s, &delta_block.vector);

    //	if(debug)
    //	{
    //		printf("\n *** Corrector Step ***\n");
    //		printf("the right-hand side:\n");
    //		for(i=0; i<state->r->size; i++)
    //			printf("r[%lu]=%f ",i,gsl_vector_get(state->r,i));
    //		printf("\n");
    //		for(i=0; i<delta->size; i++)
    //			printf("%6.3f ",gsl_vector_get(delta,i));
    //		printf("\n");
    //		printf("\ndelta_s\n");
    //		for(i=0; i<delta_s->size; i++)
    //			printf("%6.3f ",gsl_vector_get(delta_s,i));
    //		printf("\n");
    //		printf("the stepsize for the corrector step=%f\n",alpha);
    //	}

    /* Gondzio's centrality correction steps */

    i = 0;
    while (i < state->k_max) {
        alpha_gondzio = GSL_MIN_DBL(alpha + state->delta_alpha, 1.0);
        r_block = gsl_vector_subvector(state->r, cqp->Q->size1 + cqp->A->size1,
                                       cqp->C->size1);

        for (j = 0; j < z->size; j++) {
            tmp_v =
                (gsl_vector_get(z, j) -
                 alpha_gondzio *
                     gsl_vector_get(delta, cqp->Q->size1 + cqp->A->size1 + j)) *
                (gsl_vector_get(state->s, j) +
                 alpha_gondzio * gsl_vector_get(delta_s, j));
            tmp_vt = GSL_MIN_DBL(
                GSL_MAX_DBL(tmp_v, state->beta_min * sigma * (*gap)),
                state->beta_max * sigma * (*gap));
            gsl_vector_set(r_zs, j, gsl_vector_get(r_zs, j) - (tmp_vt - tmp_v));
            gsl_vector_set(&r_block.vector, j,
                           gsl_vector_get(&r_block.vector, j) +
                               (tmp_vt - tmp_v) / gsl_vector_get(z, j));
        }

        gsl_linalg_LU_solve(state->kkt_matrix, p, state->r, delta_gondzio);
        for (j = 0; j < delta_s->size; j++) {
            gsl_vector_set(delta_s_gondzio, j,
                           (-gsl_vector_get(r_zs, j) +
                            gsl_vector_get(delta_gondzio,
                                           cqp->Q->size2 + cqp->A->size1 + j) *
                                gsl_vector_get(state->s, j)) /
                               gsl_vector_get(z, j));
        }

        /* evaluate the stepsize */
        delta_block = gsl_vector_subvector(
            delta_gondzio, cqp->Q->size1 + cqp->A->size1, cqp->C->size1);
        alpha_gondzio = step_length(state->s, z, delta_s, &delta_block.vector);

        if (alpha_gondzio >= alpha + state->gamma * state->delta_alpha) {
            i++;
            alpha = alpha_gondzio;
            gsl_blas_dcopy(delta_gondzio, delta);
            gsl_blas_dcopy(delta_s_gondzio, delta_s);
        } else
            break;
    }

    /* heuristic for step length */
    alpha = GSL_MIN_DBL(0.995 * alpha, 1.0);

    /* Update */
    /* x^k = x^k + alpha*delta_x */
    delta_block = gsl_vector_subvector(delta, 0, cqp->Q->size1);
    gsl_blas_daxpy(alpha, &delta_block.vector, x);

    /* y^k = y^k - alpha*(-delta_y) */
    if (y != 0) {
        delta_block = gsl_vector_subvector(delta, cqp->Q->size1, cqp->A->size1);
        gsl_blas_daxpy(-alpha, &delta_block.vector, y);
    }

    /* z^k = z^k - alpha*(-delta_z) */
    delta_block = gsl_vector_subvector(delta, cqp->Q->size1 + cqp->A->size1,
                                       cqp->C->size1);
    gsl_blas_daxpy(-alpha, &delta_block.vector, z);

    /* s^k = s^k + alpha*(delta_s) */
    gsl_blas_daxpy(alpha, delta_s, state->s);

    /* duality gap */
    gsl_blas_ddot(z, state->s, gap);
    *gap /= cqp->C->size1;

    /* data for the next iteration */
    compute_residuals(cqp, x, y, z, state->s, state->r);
    *residuals_norm = gsl_vector_max_norm(state->r);

    build_kkt_matrix(cqp, z, state->s, state->kkt_matrix);

    /* for the infeasibility test */
    *infeasibility =
        (*residuals_norm + compute_gap_infeasible_points(cqp, x, y, z)) /
        (state->data_norm);
    *infeasibility_min = GSL_MIN_DBL(*infeasibility, *infeasibility_min);

    //	if(debug)
    //	{
    //		printf("current iteration points\n");
    //		print_vectors(x, y, z, state->s);
    //
    //		printf("\nduality gap: %e\n",*gap);
    //	}

    gsl_permutation_free(p);
    gsl_vector_free(delta);
    gsl_vector_free(delta_gondzio);
    gsl_vector_free(delta_s_gondzio);
    gsl_vector_free(delta_s);
    gsl_vector_free(r_zs);

    return GSL_SUCCESS;
}

static void mg_pdip_free(void *vstate) {
    mg_pdip_state *state = (mg_pdip_state *)vstate;

    gsl_vector_free(state->s);
    gsl_matrix_free(state->kkt_matrix);
    gsl_vector_free(state->r);
}

/*
static int
mg_pdip_restart (void *vstate)
{
  mg_pdip_state *state = (mg_pdip_state *) vstate;

  return GSL_SUCCESS;
}
*/

static int compute_residuals(const gsl_cqp_data *cqp, const gsl_vector *x,
                             const gsl_vector *y, const gsl_vector *z,
                             const gsl_vector *s, gsl_vector *r) {
    gsl_vector_view r_block;

    /*gsl_cqp_geconstraints * constraints = (gsl_cqp_geconstraints *)
     * cqp->constraints;*/

    /* r_Q=Qx+q-A^ty-C^tz */
    r_block = gsl_vector_subvector(r, 0, cqp->Q->size1);
    gsl_blas_dcopy(cqp->q, &r_block.vector);
    gsl_blas_dsymv(CblasUpper, 1.0, cqp->Q, x, 1.0, &r_block.vector);
    /*gsl_blas_dgemv(CblasNoTrans, 1.0, cqp->Q, x, 1.0, r_Q);*/
    if (cqp->A->size1 != 0) {
        gsl_blas_dgemv(CblasTrans, -1.0, cqp->A, y, 1.0, &r_block.vector);
    }
    gsl_blas_dgemv(CblasTrans, -1.0, cqp->C, z, 1.0, &r_block.vector);

    /* r_A=Ax-b */
    if (cqp->A->size1 != 0) {
        r_block = gsl_vector_subvector(r, cqp->Q->size1, cqp->A->size1);
        gsl_blas_dcopy(cqp->b, &r_block.vector);
        gsl_blas_dgemv(CblasNoTrans, 1.0, cqp->A, x, -1.0, &r_block.vector);
    }

    /* r_C=Cx-s-d */
    r_block =
        gsl_vector_subvector(r, cqp->Q->size1 + cqp->A->size1, cqp->C->size1);
    gsl_blas_dcopy(s, &r_block.vector);
    gsl_blas_daxpy(1.0, cqp->d, &r_block.vector);
    gsl_blas_dgemv(CblasNoTrans, 1.0, cqp->C, x, -1.0, &r_block.vector);

    return GSL_SUCCESS;
}

static int build_kkt_matrix(const gsl_cqp_data *cqp, const gsl_vector *z,
                            const gsl_vector *s, gsl_matrix *kkt_matrix) {
    size_t i;

    gsl_matrix_view kkt_block;

    /*KKT - Matrix

          |Q A^t    C^t  |
kkt_matrix =|A  0      0   |
          |C  0  -Z^{-1}S|

*/
    /* 1. Block */
    kkt_block =
        gsl_matrix_submatrix(kkt_matrix, 0, 0, cqp->Q->size1, cqp->Q->size2);
    gsl_matrix_memcpy(&kkt_block.matrix, cqp->Q);

    /* 2. Block */
    if (cqp->A->size1 != 0) {
        kkt_block = gsl_matrix_submatrix(kkt_matrix, 0, cqp->Q->size2,
                                         cqp->A->size2, cqp->A->size1);
        gsl_matrix_transpose_memcpy(&kkt_block.matrix, cqp->A);
    }

    /* 3. Block */
    kkt_block =
        gsl_matrix_submatrix(kkt_matrix, 0, cqp->Q->size2 + cqp->A->size1,
                             cqp->C->size2, cqp->C->size1);
    gsl_matrix_transpose_memcpy(&kkt_block.matrix, cqp->C);

    /* 4. Block */
    if (cqp->A->size1 != 0) {
        kkt_block = gsl_matrix_submatrix(kkt_matrix, cqp->Q->size1, 0,
                                         cqp->A->size1, cqp->A->size2);
        gsl_matrix_memcpy(&kkt_block.matrix, cqp->A);
    }

    /* 5. Block */
    kkt_block = gsl_matrix_submatrix(kkt_matrix, cqp->Q->size1 + cqp->A->size1,
                                     0, cqp->C->size1, cqp->C->size2);
    gsl_matrix_memcpy(&kkt_block.matrix, cqp->C);

    /* Null Block */
    kkt_block = gsl_matrix_submatrix(kkt_matrix, cqp->Q->size1, cqp->Q->size2,
                                     cqp->A->size1 + cqp->C->size1,
                                     cqp->A->size1 + cqp->C->size1);
    gsl_matrix_set_zero(&kkt_block.matrix);

    /* 6. Block */
    for (i = cqp->Q->size1 + cqp->A->size1; i < kkt_matrix->size1; i++) {
        gsl_matrix_set(
            kkt_matrix, i, i,
            -gsl_vector_get(s, i - (cqp->Q->size1 + cqp->A->size1)) /
                gsl_vector_get(z, i - (cqp->Q->size1 + cqp->A->size1)));
    }

    return GSL_SUCCESS;
}

double step_length(const gsl_vector *s, const gsl_vector *z,
                   const gsl_vector *delta_s, const gsl_vector *delta_z) {
    double alpha = 1.0;
    size_t i;

    for (i = 0; i < delta_s->size; i++) {
        if (gsl_vector_get(delta_z, i) > 0.0)
            alpha = GSL_MIN_DBL(
                alpha, gsl_vector_get(z, i) / gsl_vector_get(delta_z, i));

        if (gsl_vector_get(delta_s, i) < 0.0)
            alpha = GSL_MIN_DBL(
                alpha, -gsl_vector_get(s, i) / gsl_vector_get(delta_s, i));
    }

    return alpha;
}

static double compute_gap_infeasible_points(const gsl_cqp_data *cqp,
                                            const gsl_vector *x,
                                            const gsl_vector *y,
                                            const gsl_vector *z) {
    double g, tmp_d;

    gsl_vector *tmp_v = gsl_vector_alloc(cqp->q->size);

    gsl_blas_dcopy(cqp->q, tmp_v);
    gsl_blas_dsymv(CblasUpper, 1.0, cqp->Q, x, 1.0, tmp_v);
    gsl_blas_ddot(x, tmp_v, &g);

    if (cqp->A->size1 != 0) {
        gsl_blas_ddot(cqp->b, y, &tmp_d);
        g -= tmp_d;
    }

    gsl_blas_ddot(cqp->d, z, &tmp_d);
    g -= tmp_d;

    gsl_vector_free(tmp_v);

    return g;
}

double gsl_matrix_max_norm(const gsl_matrix *M) {
    size_t i, j;
    double max_norm = 0.0;

    for (i = 0; i < M->size1; i++)
        for (j = 0; j < M->size2; j++)
            max_norm = GSL_MAX_DBL(max_norm, fabs(gsl_matrix_get(M, i, j)));

    return max_norm;
}

double gsl_vector_max_norm(const gsl_vector *v) {
    size_t i;
    double max_norm = fabs(gsl_vector_get(v, 0));

    for (i = 1; i < v->size; i++)
        max_norm = GSL_MAX_DBL(max_norm, fabs(gsl_vector_get(v, i)));

    return max_norm;
}

/*
static void
print_vectors(const gsl_vector * x, const gsl_vector * y, const gsl_vector * z,
const gsl_vector * s)
{
        size_t i;

        printf("\nx[1 x %lu]:\n",x->size);
        for(i=0; i<x->size; i++)
                printf("%f  ",gsl_vector_get(x,i));
        printf("\n");

        if( y != NULL ) {
          printf("\ny[1 x %lu]: (LM zu Ax=b)\n",y->size);
          for(i=0; i<y->size; i++)
            printf("%f  ",gsl_vector_get(y,i));
          printf("\n");
        }

        printf("\nz[1 x %lu]: (LM zu Cx>=d)\n",z->size);
        for(i=0; i<z->size; i++)
                printf("%f  ",gsl_vector_get(z,i));
        printf("\n");

        printf("\ns[1 x %lu]: (Slack zu Cx>=d)\n",s->size);
        for(i=0; i<s->size; i++)
                printf("%f  ",gsl_vector_get(s,i));
        printf("\n");

}
*/

static const gsl_cqpminimizer_type mg_pdip_type = {
    "mg_pdip", /* name of the method: Mehrotra-Gondzio primal-dual interior
                  point method*/
    sizeof(mg_pdip_state), &mg_pdip_alloc, &mg_pdip_set, &mg_pdip_iterate,
    /*  &mg_pdip_restart, */
    &mg_pdip_free};

const gsl_cqpminimizer_type *gsl_cqpminimizer_mg_pdip = &mg_pdip_type;
